'''
Created on Dec 22, 2010

@author: eos
'''
import numpy
import math

class CheckObservability():
  
    @classmethod
    def sim_test(self, sensorPos, sensorOri, beam, measurement):
        inside=CheckObservability.test(sensorPos, sensorOri, beam, measurement)
        return inside

    def meas_test(SAS, measurement):
        sensorPos=SAS.getPosition()
        sensorOri=SAS.getOrientation()
        [xmin, xmax, ymin, ymax, zmin, zmax]=SAS.sensorProperties.getBeamMinMax()
        error("Hier fehlt noch was")
        meas_test = staticmethod(meas_test)

    @classmethod
    def test(self, sensorPos, sensorOri, beam, measurement):
        T = numpy.matrix([[1, 0, 0, 0],
                          [-sensorPos[0], 1, 0, 0],
                          [-sensorPos[1], 0, 1, 0 ],
                          [-sensorPos[2], 0, 0, 1]])
            
        sina = math.sin(-sensorOri[2] * math.pi / 180)
        cosa = math.cos(-sensorOri[2] * math.pi / 180)
        Rz = numpy.matrix([[1, 0, 0, 0],
                           [0, cosa, -sina, 0],
                           [0, sina, cosa, 0 ],
                           [0, 0, 0, 1]])
    
        sina = math.sin(-sensorOri[1] * math.pi / 180)
        cosa = math.cos(-sensorOri[1] * math.pi / 180)
        Ry = numpy.matrix([[1, 0, 0, 0],
                           [0, cosa, 0, sina],
                           [0, 0, 1, 0],
                           [0, -sina, 0, cosa]])
    
        sina = math.sin(-sensorOri[0] * math.pi / 180)
        cosa = math.cos(-sensorOri[0] * math.pi / 180)
        Rx = numpy.matrix([[1, 0, 0, 0],
                           [0, 1, 0, 0],
                           [0, 0, cosa, -sina],
                           [0, 0, sina, cosa]])
            
        meas = numpy.matrix([[1],
                            [measurement[0]],
                            [measurement[1]],
                            [measurement[2]]])
        A = Rz * Ry * Rx * T       
        result = A * meas
        measurement_T= result[1:]
        
        [xmin, xmax, ymin, ymax, zmin, zmax]=beam
        ## y
        m_y_above=( ymax- ymin)/( xmax- xmin);
        n_y_above= ymin-( ymax- ymin)/( xmax- xmin)* xmin;
        
        m_y_below=(- ymax+ ymin)/( xmax- xmin);
        n_y_below=-n_y_above   
        
        ## Z
        m_z_above=( zmax- zmin)/( xmax- xmin);
        n_z_above= zmin-( zmax- zmin)/( xmax- xmin)* xmin;
        
        m_z_below=(- zmax+ zmin)/( xmax- xmin);
        n_z_below=-n_z_above;
        
        points_above_y=m_y_above*measurement_T[0]+n_y_above;
        points_below_y=m_y_below*measurement_T[0]+n_y_below;
        
        points_above_z=m_z_above*measurement_T[0]+n_z_above;
        points_below_z=m_z_below*measurement_T[0]+n_z_below;
        
        inside=1
        if measurement_T[0]>xmax or measurement_T[0]<xmin:
            inside=0;
        if measurement_T[1]>points_above_y or measurement_T[1]<points_below_y:
            inside=0;
        if measurement_T[2]>points_above_z or measurement_T[2]<points_below_z:
            inside=0;
        
        return inside
